



//ros头文件
#include <ros/ros.h>
#include <Eigen/Eigen>

//topic 相关
#include <geometry_msgs/Point.h>
#include <px4_msgs/Px4ControlCommand.h>
#include <px4_msgs/Px4State.h>
#include <px4_msgs/Px4PositionReference.h>
#include <px4_msgs/Px4AttitudeReference.h>

#include <mavros_msgs/ActuatorControl.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Int8.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>

#include <quadrotor_msgs/PositionCommand.h>

#include <iostream>

using namespace std;

#define MIN_DIS 0.1  //如果计算得到当前位置距离目标点小于0.1，则认为已经到达目标点




//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
px4_msgs::Px4ControlCommand Command_Now;                               //发送给控制模块 [px4_pose_controller.cpp]的命令
px4_msgs::Px4State _Px4State;                                                                      //无人机状态量
ros::Publisher command_pub;                                                                       //控制命令发布者 发送给控制模块 [px4_pose_controller.cpp]

quadrotor_msgs::PositionCommand _PositionCommand;     //Fast_Planner 轨迹规划节点发布的期望轨迹

geometry_msgs::PoseStamped goal;                              // 目标点位姿               

// 停止指令
std_msgs::Int8 stop_cmd; //默认处于安全状态，后期可危险情况判断程序，该变量作为是否停止动作的标志 

//std_msgs::Bool switch_on;
//std_msgs::Bool switch_off;

int flag_get_goal = 0;  //是否获取目标点
int flag_get_cmd = 0;  //是否获取fastplanner控制命令

//是否进行航偏角控制  这里默认是直接获取fastplanner规划的航偏
bool control_yaw_flag = false;


float distance_to_goal = 0;      //当前位置与目标点距离

float desired_yaw = 0;  //[rad]   到达目标点后的期望航偏


struct fast_planner
{
    px4_msgs::Px4PositionReference fast_planner_cmd;          // fast planner cmd

}fast_planner;

ros::Publisher fast_planner_switch_pub;

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>声 明 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void Fast_planner();
void Fast_planner_test();
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

//无人机状态量
void px4_state_cb(const px4_msgs::Px4State::ConstPtr& msg)
{   
    _Px4State = *msg;
    //更新无人机与目标点距离
    distance_to_goal = sqrt(  pow(_Px4State.position[0] - goal.pose.position.x, 2) 
                            + pow(_Px4State.position[1] - goal.pose.position.y, 2) );
}

//订阅目标点--rviz 2D Nav Goal
void goal_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    goal = *msg;
    //获取目标点，相应标志设为1
    flag_get_goal = 1;
    // 重新设定目标点后,认为当前无人机不危险
    stop_cmd.data = 0;
    cout << "Get a new goal!"<<endl;
}


void fast_planner_cmd_cb(const px4_msgs::Px4PositionReference::ConstPtr &msg)
{
    flag_get_cmd = 1;
    fast_planner.fast_planner_cmd = *msg;
    cout << "Get fastplanner position command!"<<endl;
}

void planner_cmd_cb(const quadrotor_msgs::PositionCommand::ConstPtr &msg)
{
    flag_get_cmd = 1;
    _PositionCommand = *msg;
    cout << "Get fastplanner position command!"<<endl;
}



//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "planning_mission");
    ros::NodeHandle nh("~");

     //【订阅】无人机当前状态
    ros::Subscriber px4_state_sub = nh.subscribe<px4_msgs::Px4State>("/px4/state", 10, px4_state_cb);

    ros::Subscriber fast_planner_sub   =    nh.subscribe<px4_msgs::Px4PositionReference>("/px4/planning/position_cmd", 50, fast_planner_cmd_cb);
    //ros::Subscriber stop_cmd_sub = nh.subscribe<std_msgs::Int8>("/prometheus/planning/stop_cmd", 10, stop_cmd_cb);  
    ros::Subscriber fast_planner_sub_test   =    nh.subscribe<quadrotor_msgs::PositionCommand>("/planning/pos_cmd", 50, planner_cmd_cb);

    //【订阅】目标点
    ros::Subscriber goal_sub = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10, goal_cb);


     // 【发布】发送给控制模块 [px4_pose_controller.cpp]的命令
    command_pub = nh.advertise<px4_msgs::Px4ControlCommand>("/px4/control_command", 10);

    //固定的浮点显示
    cout.setf(ios::fixed);
    //setprecision(n) 设显示小数精度为n位
    cout<<setprecision(4);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    stop_cmd.data = 0; //默认处于安全状态

    // Waiting for input
    int start_flag = 0;
    while(start_flag == 0)
    {
        cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Local Planning Mission<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
        cout << "Please choose the planning method: 1 for Fast planner, 2 for ......., 3 for ........"<<endl;
        cin >> start_flag;

        if (start_flag == 1)
        {
            cout << "Fast-Planner Now" <<endl;
            //fast_planner_switch_pub.publish(switch_on);
        }else if (start_flag == 2)
        {
            cout << "Error :Please choose 1" <<endl;
            //global_planner_switch_pub.publish(switch_on);
        }else if (start_flag == 3)
        {
            cout << "Error :Please choose 1" <<endl;
            //local_planner_switch_pub.publish(switch_on);
        }
        else
        {
            cout << "Error :Please choose 1" <<endl;
        }
    }
    //控制逻辑：键盘输入后，无人机先起飞到设定高度，等待目标点
    //先起飞
    Command_Now.Command_ID = 1;
    Command_Now.source = "planning_mission";

    //设置为Takeoff模式后，等待3s飞机超过此高度即执行后面指令
    //注意：定位模块初始化后初始定位的z值若大于0.3，会导致这里进不去循环无法起飞
    while(_Px4State.position[2] < 0.3)    
    {
        Command_Now.header.stamp = ros::Time::now();
        Command_Now.Mode  = px4_msgs::Px4ControlCommand::Idle;  //怠速模式？
        Command_Now.Command_ID = Command_Now.Command_ID + 1;
        Command_Now.source = "planning_mission";
        Command_Now.Px4_Reference_State.yaw_ref = 999;    //这时切换OFFBOARD模式，发送给飞控的期望位姿是什么？
        command_pub.publish(Command_Now);   
        cout << "Switch to OFFBOARD and arm ..."<<endl;
        ros::Duration(3.0).sleep();
        
        Command_Now.header.stamp = ros::Time::now();
        Command_Now.Mode = px4_msgs::Px4ControlCommand::Takeoff;
        Command_Now.Command_ID = Command_Now.Command_ID + 1;
        Command_Now.source = "planning_mission";
        command_pub.publish(Command_Now);
        cout << "Takeoff ..."<<endl;
        ros::Duration(3.0).sleep();

        ros::spinOnce();
    }


    while (ros::ok())
    {  
        //回调
        ros::spinOnce();

        cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>> Planning Mission <<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;

        if( flag_get_cmd == 0)  //未收到fast-planner的轨迹控制指令，一直循环等待，此时飞机状态为Takeoff
        {
            cout << "Waiting for trajectory" << endl;
        }else if (distance_to_goal < MIN_DIS)  
        {
            // 抵达目标附近，则停止速度控制，改为位置控制
            Command_Now.header.stamp = ros::Time::now();
            Command_Now.Mode                                =  px4_msgs::Px4ControlCommand::Move;
            Command_Now.Command_ID                          = Command_Now.Command_ID + 1;
            Command_Now.source = "planning_mission";
            Command_Now.Px4_Reference_State.Move_mode           = px4_msgs::Px4PositionReference::XYZ_POS;
            Command_Now.Px4_Reference_State.Move_frame          = px4_msgs::Px4PositionReference::ENU_FRAME;
            Command_Now.Px4_Reference_State.position_ref[0]     = goal.pose.position.x;
            Command_Now.Px4_Reference_State.position_ref[1]     = goal.pose.position.y;
            Command_Now.Px4_Reference_State.position_ref[2]     = goal.pose.position.z;

            Command_Now.Px4_Reference_State.yaw_ref             = desired_yaw;
            command_pub.publish(Command_Now);
            cout << "Arrived the goal, waiting for a new goal... " << endl;
            cout << "px4_pos: " << _Px4State.position[0] << " [m] "<< _Px4State.position[1] << " [m] "<< _Px4State.position[2] << " [m] "<<endl;
            cout << "goal_pos: " << goal.pose.position.x << " [m] "<< goal.pose.position.y << " [m] "<< goal.pose.position.z << " [m] "<<endl;
            flag_get_goal = 0;    //一次导航结束，将目标点标志清0
            while (flag_get_goal == 0)  //循环等待下一次目标点
            {
                ros::spinOnce();
                ros::Duration(0.05).sleep();
            }     
        }else if(stop_cmd.data == 1)   //认为当前不安全  ，目前程序中stop_cmd.data一直为0
        {
            Command_Now.header.stamp = ros::Time::now();
            Command_Now.Mode                                = px4_msgs::Px4ControlCommand::Hold;
            Command_Now.Command_ID                          = Command_Now.Command_ID + 1;
            Command_Now.source = "planning_mission";

            command_pub.publish(Command_Now);
            cout << "Dangerous! Hold there." << endl; 
            ROS_INFO("Dangerous! Hold there."); 
            ros::Duration(0.1).sleep();
        }else   //正常移动
        {   
            // 运动阶段，执行规划指令  目前就一个路径规划算法-fastplanner  后续可添加
            if (start_flag == 1)
            {
                //Fast_planner();
                Fast_planner_test();   
                 
            }else if (start_flag == 2)
            {
                ROS_INFO("Only Fastplanner."); 
            }else if (start_flag == 3)
            {
                ROS_INFO("Only Fastplanner."); 
            }else
            {
                ROS_INFO("Only Fastplanner."); 
            }
        }
    }  //主循环

    return 0;
}
//该函数使用需要修改fastplanner的轨迹规划节点，使其发布我们自定义的消息类型
void Fast_planner()
{
    if (control_yaw_flag)  //这里若为ture，则自定义航偏角的控制策略
    {
        // 根据速度大小决定是否更新期望偏航角， 更新采用平滑滤波的方式，系数可调
        // fastplanner航向策略仍然可以进一步优化
        if( sqrt(fast_planner.fast_planner_cmd.velocity_ref[1]*fast_planner.fast_planner_cmd.velocity_ref[1]
                 + fast_planner.fast_planner_cmd.velocity_ref[0]*fast_planner.fast_planner_cmd.velocity_ref[0])  >  0.05  )
        {
            float next_desired_yaw_vel      = atan2( fast_planner.fast_planner_cmd.velocity_ref[1] , 
                                                fast_planner.fast_planner_cmd.velocity_ref[0]);
            float next_desired_yaw_pos      = atan2( fast_planner.fast_planner_cmd.position_ref[1] - _Px4State.position[1],
                                                fast_planner.fast_planner_cmd.position_ref[0] - _Px4State.position[0]);

            if(next_desired_yaw_pos > 0.8)
            {
                next_desired_yaw_pos = 0.8;
            }
            if(next_desired_yaw_pos < -0.8)
            {
                next_desired_yaw_pos = -0.8;
            }

            desired_yaw = (0.92*desired_yaw + 0.04*next_desired_yaw_pos + 0.04*next_desired_yaw_vel );
        }
    }else
    {
        //使用fastplanner规划的yaw
        desired_yaw = fast_planner.fast_planner_cmd.yaw_ref;
        //desired_yaw = 0.0;
    }

    Command_Now.header.stamp = ros::Time::now();
    Command_Now.Mode                                = px4_msgs::Px4ControlCommand::Move;
    Command_Now.Command_ID                          = Command_Now.Command_ID + 1;
    Command_Now.source =  "planning_mission";
    Command_Now.Px4_Reference_State = fast_planner.fast_planner_cmd;
    //下面命令当根据速度计算航偏角时才起作用
    Command_Now.Px4_Reference_State.yaw_ref = desired_yaw;

    command_pub.publish(Command_Now);
    cout << "Fast planner:"<<endl;
    cout << "desired_point: "   << Command_Now.Px4_Reference_State.position_ref[0] << " [m] "
                                << Command_Now.Px4_Reference_State.position_ref[1] << " [m] "
                                << Command_Now.Px4_Reference_State.position_ref[2] << " [m] "<<endl;  
    cout << "desired_yaw: " << desired_yaw / M_PI * 180 << " [deg] "<<endl;
    cout << "drone_pos: " << _Px4State.position[0] << " [m] "<< _Px4State.position[1] << " [m] "<< _Px4State.position[2] << " [m] "<<endl;
    cout << "goal_pos: " << goal.pose.position.x << " [m] "<< goal.pose.position.y << " [m] "<< goal.pose.position.z << " [m] "<<endl;
}

//该函数使用fastplanner 轨迹规划节点自定义的消息类型
void Fast_planner_test()
{
 
    //使用fastplanner规划的yaw
    desired_yaw = _PositionCommand.yaw;
    //desired_yaw = 0.0

    Command_Now.header.stamp = ros::Time::now();
    Command_Now.Mode                                = px4_msgs::Px4ControlCommand::Move;
    Command_Now.Command_ID                          = Command_Now.Command_ID + 1;
    Command_Now.source =  "planning_mission";
    Command_Now.Px4_Reference_State.position_ref[0] = _PositionCommand.position.x;
    Command_Now.Px4_Reference_State.position_ref[1] = _PositionCommand.position.y;
    Command_Now.Px4_Reference_State.position_ref[2] = _PositionCommand.position.z;
    Command_Now.Px4_Reference_State.velocity_ref[0] = _PositionCommand.velocity.x;
    Command_Now.Px4_Reference_State.velocity_ref[1] = _PositionCommand.velocity.y;
    Command_Now.Px4_Reference_State.velocity_ref[2] = _PositionCommand.velocity.z;

    command_pub.publish(Command_Now);
   
    cout << "Fast planner:"<<endl;
    cout << "desired_point: "   << Command_Now.Px4_Reference_State.position_ref[0] << " [m] "
                                << Command_Now.Px4_Reference_State.position_ref[1] << " [m] "
                                << Command_Now.Px4_Reference_State.position_ref[2] << " [m] "<<endl;  
    cout << "desired_yaw: " << desired_yaw / M_PI * 180 << " [deg] "<<endl;
    cout << "drone_pos: " << _Px4State.position[0] << " [m] "<< _Px4State.position[1] << " [m] "<< _Px4State.position[2] << " [m] "<<endl;
    cout << "goal_pos: " << goal.pose.position.x << " [m] "<< goal.pose.position.y << " [m] "<< goal.pose.position.z << " [m] "<<endl;
}



















